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Abstract: This paper describes a relative localization system used to achieve the 
navigation of a convoy of robotic units in indoor environments. This positioning system is 
carried out fusing two sensorial sources: (a) an odometric system and (b) a laser scanner 
together with artificial landmarks located on top of the units. The laser source allows one 
to compensate the cumulative error inherent to dead-reckoning; whereas the odometry 
source provides less pose uncertainty in short trajectories. A discrete Extended Kalman 
Filter, customized for this application, is used in order to accomplish this aim under real 
time constraints. Different experimental results with a convoy of Pioneer P3-DX units 
tracking non-linear trajectories are shown. The paper shows that a simple setup based on 
low cost laser range systems and robot built-in odometry sensors is able to give a high 
degree of robustness and accuracy to the relative localization problem of convoy units for 
indoor applications. 

Keywords: Kalman filter; sensor fusion; intelligent robots; data processing; robot control; 
laser application; dead reckoning; state estimation; multirobot system; robot sensing 
system 
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1. Introduction 

In the past, mobile robot cooperation has been widely studied in multiple application scenarios. 
Multi-robot systems exhibit advantages with respect to single-robot systems, in terms of flexibility, 
adaptability, scalability and aflbrdability. However, localization, communication and control 
challenges are more significant in cooperative robotics. 

From the point of view of localization, two kinds of scenarios can be considered: the first one 
requires a global localization of each unit independently (e.g., swarm applications [1]). In the second 
one, only relative localization between robots is required (e.g., convoy applications [2], where only 
convoy leaders may need global localization). 

The selection of the sensorial systems needed for robot localization is a crucial task that depends 
heavily on the application scenario {i.e., indoor or outdoor environments). Localization in outdoor 
scenarios can be easily performed by a combination of GPS systems and relative localization sensors 
(e.g., mid-range laser scanners, odometry, etc.). On the contrary, localization in indoor environments is 
a challenging and still unsolved problem in some aspects. Indoor GPS systems using a wide variety of 
sensor technologies (e.g., vision, ultrasound, infrared, etc.) are mostly in the research state. This paper 
deals with the problem of relative localization for cooperative guidance of robotic units in a convoy, 
considering non-linear trajectories in indoor environments. 

Multi- sensory strategies are usually proposed to solve the relative localization problem, where 
odometry information {i.e., originally included in most of the robots and prone to add cumulative 
errors), is combined with other sensors, such as laser, ultrasound or vision. In general, the accuracy of 
these technologies is highly dependent on the sensor setup. Considering only sensors onboard the 
robots, the localization accuracy depends on several factors, such as cost, number of sensors, 
complexity and limitations of each technology. This paper proposes to include a sensor on top of each 
robot that is able to give position and orientation of the next robot unit in a robotic convoy. In this 
context laser rangefinder accuracy is higher than the one based on sonar (ultrasound) [3,4], using either 
natural shape of the robot or with artificial landmarks on it. On the other hand, computational vision is 
able to easily improve the laser accuracy using visual landmarks [5] at a relative low cost. However the 
cost of the setup and its complexity increases when it is necessary to make the system resistant to 
varying illumination conditions (e.g., active infrared landmarks) and to operate at high frame rates. 

This paper shows that a simple setup based on low cost laser range systems and built-in robot 
odometry sensors is able to provide a high degree of robustness and accuracy to the relative 
localization problem of convoy units in indoor environments. Aside from the localization problem, the 
design of a control strategy for each individual unit in the convoy presents important challenges. In 
order to follow the leader's trajectory it is not enough to guarantee global stability. A movement 
coordination plan is also needed between at least each pair of consecutive units [6]. This coordination 
involves exchanging continuously the motion state between convoy units, as explained in works such 
as [7-10]. To successfully achieve convoy navigation it is essential to have a highly reliable and exact 
positioning system providing the convoy leader with its global pose and each convoy unit with its 
relative localization to the preceding unit. 

Among the published works regarding convoy guidance for indoor applications, the following should 
be mentioned: In [11] a sensorial system is designed for the high level navigation of a convoy for indoor 
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construction site security and safety. The proposed on-board sensorial system in the robots (ultrasonic 
range modules, infrared distance measuring devices, colour camera, microphone and speaker) is 
complemented by wireless sensor networking devices. In [12] a vision system can recognize and 
relatively localize the follower robots using markers mounted on the leader unit. In [13] the indoor 
localization problem for convoy guidance is solved using a camera and colour signboard landmarks 
placed in the environment. A Kalman Filter and an Interacting Multiple Model method are applied to 
find the robots accurate positions and identify them by using the signboards. In [14,15] a demonstrator of 
a leader and four followers is described, where relative localization between convoy units is solved by 
means of a Sick LMS 200 laser rangefinder (LIDAR) and the robots themselves (Pioneer 2-DX) are used 
as landmarks. The Sick LIDAR sensor proposed had 0.25° of angular resolution, 15 mm of depth 
resolution and 10 m range. Although using an accurate sensor, the aforementioned work does not include 
odometry information and it thus relies only on the laser measurements. 

In the light of the previous works the main contribution of this paper is to implement an innovative 
and low cost relative localization system for a convoy of robotics units in indoor transport scenarios. 
The LIDAR sensor proposed in this paper is a Hokuyo URG-04-LX [16,17]. Its performance is 
remarkable lower (0.36° of angular resolution, 40 mm of linear accuracy and 4 m range) than the 
previous mentioned Sick LIDAR, but the cost is about six times cheaper. To compensate its accuracy 
this paper proposes to combine laser measurements with odometry. This way, the algorithm is able to 
profit from the high resolution of odometry (L2 mm of resolution) in short movements at the same 
time non-cumulative error is compensated in large trajectories using laser measurements. Besides, 
odometry sensors operate at high frequency (50 ms in the application described in this paper), which 
allows to maintain relative localization accurate whether a momentary blinding happens; that is not 
possible using only laser, camera or sonar devices. To summarize, data fusion makes it possible to 
combine the positioning data of the robot odometric system (with a low uncertainty but cumulative 
error) and an on-board laser scanner in the follower units (with non-cumulative errors). A solution 
based on the discrete Extended Kalman Filter (EKF) is proposed. Units used in the convoy demonstrator 
are based on P3-DX robots from MobileRobots [18,19], that have been adapted to the requirements of 
the proposed scenario with different electronic devices (see Figures 1 and 2); some of them were 
designed ad-hoc [20,21]. 

Figure 1. One of the P3-DX robotic platforms equipped with encoders and laser scanner 
for the platoon guidance demonstrator. 
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Figure 2. Convoy of three robotic units as the one shown in Figure 1. Landmarks related to 
the laser scanner system are onboarded on top of the robots. 




As described in detail in [2,6,7,21], the success of the guidance task in platooning applications 
strongly depends on the relationship between control, communication and sensorial systems. 
Regarding control and communication solutions for platooning guidance in hard non-linear trajectories 
different works have been carried out by the authors in the context of the COVE project [22]. The 
global control architecture [2] for each follower of the convoy includes a three level controller, as 
shown in Figure 3. 

Figure 3. Organization of the control levels and their relation with the sensorial systems 
included in each follower unit. 
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The low level is based on a set of FID controllers that regulate the speed of each active wheel. The 
middle level includes a servo-controller in order to ensure reliable angular and linear speeds (Vo) of 
the robot. The robotic units are provided with optical encoders of 500 pulses per revolution linked to 
each active wheel, due to its 19 cm diameter the movement resolution is 1.2 mm. In this way, 
odometry permits closing both the low and middle control loops. Additionally, a discrete Kalman 
Filter (KF estimator) is included in order to filter the noise related to measurements provided by 
encoders and obtaining the filtered velocity vector (VE) for control purposes. The high level generates 
the inputs (UHL) for the middle level, such that each robot follows the previous one warranting a 
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security distance, and approaches the discretized poses selected by the leader in its way. The follower 
pose, needed to accomplish the high control level objectives, is estimated by the discrete EKF 
combining data coming from the odometric system and the added laser scanner. 

The paper is organized into five sections: after this introduction, the sensory sources involved in the 
fiision process are presented in Section 2, and the discrete EKF application to the data fiision data is 
described in Section 3. Experimental results obtained with a real robotic convoy demonstrator are 
shown in Section 4, and conclusions are revealed in Section 5. The mathematical component and 
algorithm description are included in an appendix at the end of the paper. 

2. Pose Estimation of Follower Units based on Odometry and Laser Combination 

The convoy consists of a group of robotic units that are only equipped with relative positioning 
sensors {i.e., odometry and laser sensors). One of the units is designated as the leader unit and it is 
assumed that it knows its global position in the environment. Besides, all the units are nodes of a 
wireless local area network (Wi-Fi link) [2]. 

As already mentioned, global stability is achieved if each follower knows both its motion state and 
at least the one of the precedent unit. In order to accomplish this specification, as a first approximation, 
angular and linear speeds of each robot in the platoon can be estimated with a dead-reckoning process, 
using the encoders attached to active wheels already built on each robot. This estimation can be 
combined between each pair of robots, and sent through the wireless link, to obtain each unit's relative 
distance and orientation to the precedent one. However, this first approach produces important drifts in 
the pose estimation, due to the accumulative error inherent to the dead-reckoning process, mainly in 
non-linear trajectories. A complementary sensorial system is therefore needed in order to better 
estimate the individual pose and to guarantee the reliability and stability in the guidance task 
performed in the convoy. 

In this paper, authors propose to combine the odometric information with the laser sensor pose 
estimation. The laser sensor gives the relative pose (distance and orientation) from each robot to the 
preceding one (see Figures 1 and 2), thus avoiding cumulative errors in this information. Nevertheless, 
experimental results demonstrate that the uncertainty related to the pose information, calculated from 
the laser sensor, is bigger than the one obtained from the odometric one. However, it has to be taken 
into account that the error concerning the laser scanner is bounded while the one related to the 
odometric system is cumulative. Fusion strategies are therefore needed in order to compensate 
limitations and to exploit the positive characteristics related to each of the two sensory systems in the 
guidance application. 

The laser contributes to measure the separation distance d^j between units in the platoon, and to 
ascertain the correction angle 9ei needed by each unit to approach to the next pose mark PiTk sent by 
the leader to the rest of the convoy units [2]. These variables are illustrated in Figure 4. Complementing 
the laser scanner, basic artificial landmarks are placed on top of the robotic platform (see Figure 2). 
The landmark system includes two small planes and a cylinder between them, overhanging the 
compact volume of the basic platform. The cylinder is located on the dynamics reference point of the 
robot. It can be noticed in Figure 5 that two of the three elements included as artificial landmarks are 
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enough to obtain both the separation distance d^j and the relative angle between robots poses Pi 
and Pi - 1. 

Figure 4. Variables involved in the high level control of the platoon: points of the leader 
trajectory, pose of each follower and relative position information obtained through the 
laser scanner. 
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Figure 5. Description of the geometrical relation among the variables 
{di, 6i, 02, d-^i, Oci] implied in the laser scanner relative positioning system. 




However, working with the proposed landmark has important advantages: 

a. The inclusion of two planar elements minimizes the error when calculating the angle a, as the 
separation between them is big enough. The angle a is used to compute the relative orientation 
Oy^i between each two consecutive units in the platoon. 

b. Although distance d^j can be indirectly obtained through the measures {di,6i\ and {d2, 62^, the 
cylinder in the middle of the landmark eases its direct computation, improving the accuracy 
and computational time of the estimation. 

c. Thanks to its three components, the artificial landmark can be easily identified in the robot 
structure and from the different elements in the environment, minimizing the fault detections. 
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Once the artificial landmark is detected by the laser scanner on top of a follower unit, the relative 
distance between this unit and the one in front of it is directly obtained from the laser measures to the 
cylindrical structure . From the two most external measures, detected in the landmark by the laser 
scanner (points ei and 62 in Figure 5), the angle a can be calculated: 



a = atan2 



Vd2 



sin (61) - d2 sin (82 
2 cos (62)- di cos (9 



1)/ 



(1) 



where atanl is a 4-quadrant version of the inverse tangent function. 

This way, the relative orientation respect to the precedent unit is obtained by the equation: 

= T - a 



(2) 



To better understand the data fusion process, the following nomenclature should be kept in mind: X 
is the predicted pose based on odometry, Z is the estimated pose through laser measurements and X 
represents the corrected pose through the EKF algorithm. 

Thanks to the wireless link between the units, the corrected pose 
Xi_i j^_i = [xi_i j^_i yi_i of the Fi_i unit at the k-l-th instant, is known by the unit 
Fi at the k-th instant. In this way, using the relative laser measures, the estimated pose 
^i,k = [ ^i,k yi,k ^i,/c] ^ the k-th instant is obtained as shown in Equations (3-5): 

9i,k = 9i-l,k-l ~ 6ri,k (3) 

Xi,k = Xi-i,k-i - dri,kCos(ei,k + 9ci,k) (4) 
yi,k = yi-i,k-i - dri,kSin(ei,k + 9ci,k) (5) 

Finally, the EKF algorithm allows one to fuse the odometric information Xij^ with the laser one Zj 
to achieve the corrected pose X^ j^ of the Fj unit, see Figure 6. 

Figure 6. Processes and variables implied in the EKF to obtain the Fi movement state. X is 
the predicted pose (odometry system), Z is the estimated pose (laser measurements), and X 
represents the corrected pose (EKF algorithm). 
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3. Discrete EKF Application to the Odometry and Laser Fusion 



The best pose estimation of each follower in the convoy is achieved through a discrete Extended 
Kalman Filter [23-25], fusing odometry and laser scanner information. The EKF allows one to 
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highlight the strengths of the two sensory systems. Thus, the filter develops the functions shown in 
Figure 6 in two steps: 

a. Prediction of the robot pose X]^ . The odometry information is included as input vector 
U]^ according to the speed of the active wheels at each sample time. The corrected state X]^_i in 
the previous sample time is also required. 

b. Correction of the pose estimation ^/^. This step requires the estimated pose Z/^ obtained once 
the laser scanner information is achieved. 

At the end of this paper, the Appendix mathematically details the specific adaptation of the discrete 
EKF to the problem tackled in this work, which is summarized in Figure 7. 

Figure 7. Block diagram of the implemented fusion algorithm, based on the standard EKF. 
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The implemented EKF has the standard structure of this filter, except for the factor Q^. This factor 
indicates the availability of the laser scanner measure: if its measures are available in a specific time k 
then 0/^ = 1; otherwise 0/^ = 0, and the correction step will not be executed that time k. The use of 
factor allows having timing independence for prediction and correction process [26]. In this work a 
sampling time of Ts = 0.05 s is constantly used for the prediction step, meanwhile the time correction 
will vary according to the availability of the laser scanner measures, as explained. 

The different tasks developed by the filter at the EKF prediction step are summarized in the 
following paragraphs: 

(p.l) Prediction of state Xij^ (position and orientation) for the follower unit in an absolute 
positioning reference system. Dead-reckoning model based on the odometric system (f function in 
Figure 7), and the corrected state at previous time step Xij^_i , are required to obtain this 
predicted state. 

(p. 2) Estimation of measure Zj from the corrected pose of the precedent unit Xi_i j^_i and the 
measurement model based on the laser scanner (g function in Figure 7). 
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(p. 3) Prediction of tlie estimation error co variance matrix Pj , using the corrected value of tliis 
matrix for the previous time step Pt^k-i^ well as the noise co variance matrix Ey\i of the odometric 
measurements' model, and the two jacobians ]f^x ^i^d (see the Appendix). 

On the other hand, tasks developed by the discrete filter at the correction step, are the following: 

(c.l) Updating the Kalman gain Kj ^- In order to obtain this gain, some matrices have to be 
previously computed: estimation of the matrix Pj k, the noise co variance matrix of the laser scanner 
measurements' model, and the two jacobians Jg x and Jg y (see the Appendix). 

(c.2) Correction of the pose state predicted value Xj ^ , only if the laser scanner updated measures 
are available, and thus 0^ = 1. As it can be noticed in Figure 7, this correction is obtained weighting 
the difference between the position information obtained with the laser scan , and its prediction 
through odometry, with the Kalman gain. 

(c.3) Updating the estimation error covariance matrix Pj k- 

Among the contributions of this paper the standard discrete EKF adaptation for the pose estimation 
of robots in platooning guidance should be considered. Specifically the authors have developed: 

• Characterization of / and g functions. The / one is related to the dead-reckoning model used to 
obtain the position information with the odometric system. The g function concerns the positioning 
system based on the relative measures of the laser scanner and the pose of the precedent unit. 

• Computation of the covariance matrices that model the noise related to both sensory systems: the 
one related to the odometric system ly^ and the other deals with the laser scanner ly. In order to 
find these values, the angular speed of the active wheels as well as the distance and angles 
measured with the laser scanner has been registered in 50 experiments. The standard deviation of 
the related noise variables and the complete covariance matrices are computed from those registers. 

• Calculation of the needed jacobians. The jacobians depending on odometry measurements: ]f x and 
]fyv'^ and the ones regarding the laser measurements: ]g z and ]g y. 

4. Experimental Results 

In the experimental tests developed to validate the described proposal, three robotic units adapted 
from the original P3-DX platform (see Figures 1, 2 and 5) have been deployed. All of them are initially 
synchronized and currently linked by means a Wi-Fi LAN in compliance with IEEE802.11b/.llg 
standards [20]. The solutions carried out to mitigate the packet dropout effect were tackled by the 
research group in other work [27] and have also been implemented in these tests. The hardest time 
constraints are imposed by: the scan time of the Hokuyo device (100 ms) [16,17] and the velocity of 
the robot (limited to 1 m/s) [2,18]. 

Two types of tests are included in this section. The first one is dedicated to show the advantages of 
the implemented fusion technique, comparing the positioning results independently obtained with each 
of the two sensory systems under study. The second type focuses on the global results of control and 
sensorial fusion integration applied to a convoy of robots. 

In the first set of results only two units are used: the leader, programmed to track a trajectory 
including straight and curve segments; and a single follower. Figure 8 shows the follower path 
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according to the different sensorial sources but without fusion apphcation: in red it is shown the 
movement registered by the odometric sensory system; and in blue the one registered by the laser 
scanner through the relative measure respect to the leader movement. The follower unit starts at point 
[x = -1, y = 0]. Both depicted trajectories are close along its first straight part. Nevertheless, the 
information given by the two sensorial systems diverges from the moment the trajectory presents a 
curve path. Figure 9 shows the linear and angular speed registered by the odometric system of the 
follower unit tested in this first experiment. This figure allows one to demonstrate, in other way, the 
effect of the filter included to remove the odometry noise. In fact, it can be noticed that this noise is 
more relevant in the angular speed case, confirming the need of inserting the KF estimator in the 
global control solution (see Figure 3) for non-linear trajectory tracking. 

Figure 8. Movement developed by a P3-DX robotic unit following a leader. The red trace 
agrees with the odometric information of the robot. However, the laser scanner gives the 
more realistic blue path. 
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Figure 9. Linear and angular speed of the follower unit in the trajectory shown in Figure 8. 
The values registered by the odometry system are plotted in blue, and the filtered ones 
(used for control tasks) are in red. 
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A new experiment is carried out with the same robot formation and with the same path reference. 
This time, the output of the fusion algorithm is apphed to the high level control in the guiding 
architecture shown in Figure 3. In order to evaluate the discrete EKF functionality when the fusion task 
cannot be strictly performed, because of the lack of laser scanner measures, this sensor is blinded in 
some time intervals. An obstacle is inserted just in front of the scanner in some specific moments along 
the path. It can be then analyzed how the guidance application does correct the robot path from the 
drift typically generated by the stand alone use of odometry once the obstacle disappears. Figure 10 shows 
the path pursuit by the follower unit, using the global fusion algorithm here proposed (red line o); the 
laser scanner measures are also plotted in blue, when available. In this path, along segments "ab'\ "cd" 
and "ef both sensory systems generate valid pose measures, and therefore, the fusion process is 
correctly developed. Besides, along segments "be'' and "de'\ position information is not available in 
the laser scanner system, so only the prediction step of the EKF is working just using odometric 
information. The result of this information lack of the laser scanner is that the movement of the robot 
unit presents a relevant drift from its expected path when using only odometry, mainly in curve 
intervals. In any case, once the laser scanner measures are again available for the fusion algorithm, the 
guiding process is quickly adjusted to the correct path. 



Figure 10. Path pursuit by the follower unit using the discrete EKF fusion proposal as part 
of the high level control. The blue plot shows the position information registered by the 
laser scanner, and the red one the location estimated by the EKF. The laser scanner is only 
available in intervals "ab'\ "ed" and "ef\ 
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The second type of test is developed with a convoy of three units, as shown in Figure 2, in a more 
complex scenario. A video showing the overall experiment can be seen in [22]. The platoon guidance 
strategy, based on the three control levels and the sensorial fusion algorithm described in this paper, is 
implemented in the two follower units. As it can be appreciated in Figure 1 1, the platoon starts at LOS 
laboratory (where "a", 'V and "c" are respectively the initial localization of each unit) and goes 
through a corridor to finally get into L02 laboratory (where "a' "b' " and "c' " are respectively the 
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final localization of each unit). The total path followed by each robot in this platooning guidance 
example is depicted with different colours. The robots' location is obtained from each "EKF sensor 
fusion" block (see Figure 3). It can be stated that the two follower units track with negligible error the 
trajectory described by the leader. 

Figure 11. Trajectory followed by the platoon. The path described by the leader is plotted 
in red, the one described by the first follower is shown in green, and the one described by 
the second follower is plotted in black. The reference trajectory input to the leader is also 
shown in blue. 
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5. Conclusions 



This work details how the proper combination of odometry and a low cost laser scanner provides 
the required accuracy and non-cumulative errors for indoor applications of a convoy of robotic units. 
First, it has been demonstrated that information coming from the proposed single sensors is not enough 
to accomplish the correct positioning of one or more units in cooperative guidance. In this context, the 
proposal presented in the paper calls for fusing odometric data (typical positioning system of a robot) 
with a laser scanner (added to the robotic platform together with a basic landmark structure) to achieve 
the guiding task of a convoy of P3-DX robots. 
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The contribution of the implemented discrete EKF is twofold. On one hand the inherent 
accumulative error due to dead-reckoning positioning is corrected by the laser measurements. On the 
other hand, the highest uncertainty related to the used low cost laser scanner is compensated by the 
lowest one of the P3-DX encoders. 

As it has been demonstrated with the indoor experiments results, the sensorial fusion process is 
essential to maintain a safe distance between followers and to track the leader's trajectory. The 
implemented solution allows one to achieve these objectives, even in situations where partial sensory 
information is lacking. 

In summary, the paper details quantitatively how the performance of independent sensorial sources 
can be highly improved by means of a proper fusion algorithm, taking advantage of their best 
characteristics and minimizing their inherent limitations. 
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Appendix 

Mathematical Description of the Implemented Discrete EKF Algorithm 

The kinematical relation between the robot pose and the speed data of active wheels is not linear in 
a differential drive robot. Thus, in order to represent this relation in the state space, the transition and 
output equations respectively have to be expressed as follows: 

Xi,k=f(Xi^k-i.Ui,k,Wk) (A.1) 

Zi,k = g(Xi_,^k-i.Vk) (A.2) 

where Xi j^ E E^, is the state vector representing the absolute pose of the follower unit, with its three 
components fx, 0);Xi_i]^ EM?, is the state vector of the precedent unit; EM?, is the input 
vector, with its two components: angular speed fo)/^, of the two active wheels in the platform (right 
and left); Zj/^ G M^, is the pose estimation through the laser scanner measures (distance and angle); 
Wk EM^, is the state noise vector, therefore related to the odometric system; and EM^, is the 
measurement noise vector related to the laser scanner perception system. 

As defined in the previous paragraphs, nonlinear and stochastic functions / and g are respectively 
related to the odometric system intrinsic to the robot, and to the laser scanner sensorial system. 

As explained in Section 3, and depicted in Figure 7, two steps are periodically repeated in order to 
develop the EKF sensorial fusion. In the prediction step. Equations (A.3)-(A.5) are determined. A null 
value is supposed in this step for all noise components: 

Xi,k=f(Xt,k-vUt,k^Wk) (A3) 

Zuk=g(Xt-i^k-vO) (A.4) 

P i,k = Jf,Xi P i,k-l ]},Xi + lf,W Jf,W (A.5) 

In the case under study, function /can be defined analyzing separately the three components of Xij^ , 
and thus, obtaining expressions (A.6) to (A.8) to substitute (A.3) as follows: 

% = ^a-i + ^^^^^^^ ■ Ts . cos[e,,_, + i^^M^ . Ts] (A.6) 
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+ 



{(»R,k-<^L.ky 



Ts] 



(A.7) 

(A.8) 



where Ts is the sample time of the discrete fusion process, r is the active wheels radius and D is the 
distance between them. In this specific application Ts = 0.05 s, r = 0.09 m and D = 0.33 m. 

To obtain Pj/^, the components Jf^xi ^ Jf,w are needed. Expressions (A.9) and (A. 12) define 

the two first jacobians in the context of interest: 

dxi^k d^i,k d^i.k 



Jf,Xi - 



dXi^k-1 


dyi,k-i 


dOuk-l 


dyi,k 


dyi,k 


dyi,k 




dyi,k-i 


dei,k-i 


d0i,k 


d0i,k 


dei,k 



■dxi^k-i ^yi,/c-i d^i,k-i- 



1 0 



0 1 



dyi,k 



dei,k-i 
00 1 



(A.9) 



where: 



= - (^iMJpj> . Ts ■ sin (d,,u-i + ■ Ts) 

IS • COS^Oik^i H I S) 



d0i,k-l 
d0i,k-i 



(A. 10) 
(A. 11) 



and: 







dxi^k 




dojR,k 


d0)L,k 


Jf,w — 


dyi,k 


dyi,k 




d(OL,k 










-dojR^k 


do)L,k 



(A.12) 



where the different elements of jacobian Jf ^r are obtained as follows applying the kinematics relations 
described in (A.6) to (A.8): 



= --Ts-cos 



D 



2D 



• Ts^ • sin 



D 



(A.13) 



= --Ts-cos 

do)L.k 2 



(^R.k + (^L.k)^'^ 



D 



+ 



( 



2D 



• Ts^ • sin 



D 



(A. 14) 



dKk r . 
= — -Ts ■ sin 

O^R,k 2 

2D 



D 



+ 



Ts^ ■ cos 



D 



(A.15) 
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dyi.k r . 

^ = --Ts • sin 

do)L,k 2 

2D 



5 , -<^L,fe> 

+ Ts 



D 



Ts^ • cos 



2 , i(^R,k - ^L,fc)r 
^i.k-1 + Ts 



D 



L,k 



(A.16) 

(A. 17) 
(A.18) 



Besides, the noise covariance matrix related to the odometric information I-w is empirically and 
statistically defined (see Section III), resulting in this application as follows: 

rr n 



■'enc 

0 



2/2 



, being Eenc =3.8125 106rad7s 



(A. 19) 



Once Equation (A. 3) has been detailed, we focus on tlie Equation (A.4) througli the g function. It 
includes terms of the precedent unit Fi.j and laser scanner measures, as it has been described in 
Equations (3) to (5), and has been depicted in Figure 5. The estimation pose, which is based on the 
laser sensorial system, includes three components Zi j^ = [xi^ ,yi k ,di kY that are going to be 
analyzed separately: 



0ci,k + 0i,k-i - ^ + atan2 



yi,k — yi-l,k-l ~ dri^k 

9i,k = 9i-i,k-i - 7 + atan2 



\d2,k cos{e2,k)- di,,k COS {e I k) J 



^ci,k + ^i-i,k-i atan2 



cos{e2,k)-di,,k COS {9 ^ J 
( di^k sin {Oi,k) - d-2,k sin (6*2 

Wk 



cos{e2,k)-di^k cos{e 
k sin{ei^k)-d2,k sin (6*2 



h,k)) 



(A.20) 
(A.21) 
(A.22) 



,k cos {e2,k)-dikCos{e 

At this point, the correction step is tackled. The Kalman gain Kij^, the estimation error covariance 
matrix Pj and the corrected pose X^ j^ are evaluated, as shown in Figure 7. The Kalman gain is 
obtained from the jacobians ]g x and ]gy , whose values are calculated as follows: 
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(A.23) 



where: 



,k + 0i,k) 
^ci,k + ^i,k) 



(A.24) 
(A.25) 



and: 
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(A.26) 



Expressions from (A.29) to (A.46) allow one to achieve the 18 elements of ^ , knowing that: 
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dOi,k _ -cos{e2,k)b+sen{e2,k)a . 
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(A.46) 



The noise covariance matrix regarding the laser scanner information Ey is statistically and 
empirically defined through angular and distance parameters as described in Section III. In the case 
under study, the results are: 
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0 
0 
0 

^9 



(A.47) 



where = 8.286 • lO'^ m^ and Sg = 7.615 • lO'^ rad^. 

All data generated as described in previous paragraphs are needed to correct F, pose, through 
expression (A.48). This equation performs the correction of state vector Xi j^ , from its prediction 
based on the odometric information and the pose deduced from the laser scanner measures Zi j^ : 

Xi,k = + 6>fc K,,u [Zi,u - Xa)] (A.48) 

where 0/^ essential functionality is described in Section 3. 

The correction step ends updating the error estimation covariance matrix, as follows: 

Pi.k = U-Ki,kJg.x-)Pi.k (A.49) 

being I the identity matrix. 
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